#!/usr/bin/env python

import rospy
from sensor_msgs.msg import JointState

class JointStateListener:
    def __init__(self):
        self.joint_positions = {}
        self.joint_velocities = {}
        rospy.Subscriber("/aliengo_gazebo/joint_states", JointState, self.callback)

    def callback(self, msg):
        for i, name in enumerate(msg.name):
            self.joint_positions[name] = msg.position[i]
            self.joint_velocities[name] = msg.velocity[i]

    def get_joint_states(self):         
        return self.joint_positions, self.joint_velocities

if __name__ == "__main__":
    rospy.init_node('joint_state_listener_node', anonymous=True)
    listener = JointStateListener()
    
    rate = rospy.Rate(60)  # 10 Hz
    while not rospy.is_shutdown():
        positions, velocities = listener.get_joint_states()
        rospy.loginfo("Joint Positions: {}".format(positions))
        rospy.loginfo("Joint Velocities: {}".format(velocities))
        rate.sleep()
